#include "lidar_localization/models/graph_optimizer/g2o/edge/edge_gnss.hpp"

namespace g2o
{
    EdgeGnss::EdgeGnss() : BaseMultiEdge<3, Eigen::Vector3d>()
    {
        resize(1);
    }

    void EdgeGnss::computeError()
    {
        const VertexVec *v = dynamic_cast<const VertexVec *>(_vertices[0]);
        _error = v->estimate() - _measurement;
    }

    void EdgeGnss::linearizeOplus()
    {
        _jacobianOplus[0] = Eigen::Matrix3d::Identity();
    }
} // namespace g2o